# 版权所有 2025 地球仪 (ldhhp@163.com)
# 本文件基于灯哥的 Py-Apple Dog 项目进行修改。
# 原始项目地址：https://github.com/ToanTech/py-apple-quadruped-robot
# 本项目遵循 Apache 许可证 2.0 版。
# 你可以在以下地址获取许可证副本：http://www.apache.org/licenses/LICENSE-2.0

#import dog_control_test as dog_control #pc调试web用
import dog_control
import dog_data
import dog_cfg
import socket
from wifi_moudle import selfadd
from math import floor

#-----------------------HTTP Server-----------------------#
user_leg_num="1"
url_cal="cal.html"
url_c="control.html"
thr=0;turn=0;L=0;R=0;Pitch=0;Roll=0;Yaw=0;Yst=dog_data.in_y;Hgt=110 #中间变量定义
color_leg1='#7DFF7D';color_leg2='#FF9E9E';color_leg3='#FF9E9E';color_leg4='#FF9E9E';
step_state=0;test_add=0
url_n=url_c
g_s_num=100
print("bind ip: "+selfadd)
try:
  addr = (selfadd,80) #定义socket绑定的地址，ip地址为本地，端口为80
  s = socket.socket()     #创建一个socket对象
  s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
  s.bind(addr)            #绑定地址
  s.listen(1)             #设置允许连接的客户端数量
  print('listening on:', addr)
except Exception as e:
  print('Web 初始化异常，'+str(e) )
  s=None

dog_control.gesture(Pitch,Roll,0,Yst)

while s is not None:
  try:
    cl, addr = s.accept() #接受客户端的连接请求，cl为此链接创建的一个新的scoket对象，addr客户端地址
    #print('client connected from:', addr)
    req=str(cl.recv(1024))
    cl.sendall('HTTP/1.1 200 OK\nConnection: close\nServer: FireBeetle\nContent-Type: text/html\n\n')
    req=req.split('\\r\\n')
    #http header 解析
    req_data=req[0].lstrip().rstrip().replace(' ','').lower()
    if req_data.find('favicon.ico')>-1:
      cl.close()
      continue
    else:
      req_data=req_data.replace('get/?','').replace('http/1.1','').replace("b'","")
      
      if req_data=='get/':
        url_n=url_c

      if req_data.find('speed')>-1:
        #print(req_data.replace('&',';'))
        exec(req_data.replace('&',';'))
        dog_data.legL[0]=l1
        dog_data.legL[1]=l2
        dog_data.legL[2]=l3
        dog_data.bodyL=l
        dog_data.bodyW=b
        dog_data.legW=w
        dog_data.speed=speed
        dog_data.speed_init=speed
        dog_data.raiseH=h
        dog_data.Kp_H=kp_h
        dog_data.Kp_G=kp_g
        dog_data.trot_cg_f=trot_cg_f
        dog_data.trot_cg_b=trot_cg_b
        dog_data.trot_cg_t=trot_cg_t
        dog_data.Kp_TW=kp_tw
      #判断摇杆
      index_f = req_data.find('f=')
      index_find_t = req_data.find('t=')
      value_f = req_data[index_f+2:index_find_t].lstrip().rstrip()
      index_t = req_data.find('t=')
      value_t = req_data[index_t+2:index_t+6].lstrip().rstrip()
      try:
        thr=int(value_f)*6/100
        turn=int(value_t)
      except :
        pass
      #判断按钮
      index = req_data.find('key=')
      value = req_data[index+4:index+6].lstrip().rstrip()
      #Pitch
      if req_data.find('pit=')>-1:
        index_p = req_data.find('pit=')
        value_p = req_data[index_p+4:index_p+7].lstrip().rstrip()
        if value_p!='/':
          print('pit:',str(value_p))
          Pitch=int(value_p)
      #Roll
      if req_data.find('rol=')>-1:
        index_r = req_data.find('rol=')
        value_r = req_data[index_r+4:index_r+7].lstrip().rstrip()
        if value_r!='/':
          #print('rol:',str(value_r))
          Roll=int(value_r)
      #Height
      if req_data.find('hgt=')>-1:
        index_h = req_data.find('hgt=')
        value_h = req_data[index_h+4:index_h+7].lstrip().rstrip()
        if value_h!='/':
          print('hgt:',str(value_h))
          Hgt=int(value_h)
      #Yaw
      if req_data.find('yaw=')>-1:
        index_h = req_data.find('yaw=')
        value_h = req_data[index_h+4:index_h+7].lstrip().rstrip()
        if value_h!='/':
          print('yaw:',str(value_h))
          Yaw=int(value_h)
      #Y_controller
      if req_data.find('yst=')>-1:
        index_y = req_data.find('yst=')
        value_y = req_data[index_y+4:index_y+7].lstrip().rstrip()
        if value_y!='/':
          print('yst:',str(value_y))
          Yst=int(value_y)
      #运动控制用
      elif value == 'ss':
        Pitch=0;Roll=0          #清除姿态
        dog_control.stable(False)     #清除陀螺仪
        dog_control.gait(0)           #重置步态模式
        url_n=url_cal
        step_state=0
      elif value == 'go':
        print('True')
        dog_control.stable(True)
      elif value == 'gc':
        print('False')
        dog_control.stable(False)
      elif value == 'sn':
        step_state=1
      elif value == 'sf':
        step_state=0
      elif value == 'g0':
        dog_control.stable(False)   #切换walk步态时自动关闭陀螺仪，防止冲突
        dog_control.gait(0)
      elif value == 'g1':
        dog_control.stable(False)
        dog_control.gait(2)
      #标定判断用
      if value == 'l2':
        user_leg_num='2'
        color_leg1='#FF9E9E';color_leg2='#7DFF7D';color_leg3='#FF9E9E';color_leg4='#FF9E9E'
      elif value == 'l4':
        user_leg_num='4'
        color_leg1='#FF9E9E';color_leg2='#FF9E9E';color_leg3='#FF9E9E';color_leg4='#7DFF7D'
      elif value == 'l1':
        user_leg_num='1'
        color_leg1='#7DFF7D';color_leg2='#FF9E9E';color_leg3='#FF9E9E';color_leg4='#FF9E9E'
      elif value == 'l3':
        user_leg_num='3'
        color_leg1='#FF9E9E';color_leg2='#FF9E9E';color_leg3='#7DFF7D';color_leg4='#FF9E9E'
      elif value == 'sc':   #保存并退出
        s_f = open("dog_data.py", "w+")
        #保存中位
        s_f.write("parts = [\n")
        s_f.write("    ["+str(dog_data.parts[0][0])+", "+str(dog_data.parts[0][1])+", "+str(dog_data.parts[0][2])+"],\n")
        s_f.write("    ["+str(dog_data.parts[1][0])+", "+str(dog_data.parts[1][1])+", "+str(dog_data.parts[1][2])+"],\n")
        s_f.write("    ["+str(dog_data.parts[2][0])+", "+str(dog_data.parts[2][1])+", "+str(dog_data.parts[2][2])+"],\n")
        s_f.write("    ["+str(dog_data.parts[3][0])+", "+str(dog_data.parts[3][1])+", "+str(dog_data.parts[3][2])+"]\n")
        s_f.write("]\n")
        #保存机械、步态参数
        s_f.write("legL=["+str(dog_data.legL[0])+","+str(dog_data.legL[1])+","+str(dog_data.legL[2])+"]\n")
        s_f.write("bodyL="+str(dog_data.bodyL)+"\n")
        s_f.write("bodyW="+str(dog_data.bodyW)+"\n")
        s_f.write("legW="+str(dog_data.legW)+"\n")
        s_f.write("speed="+str(dog_data.speed)+"\n")
        s_f.write("raiseH="+str(dog_data.raiseH)+"\n")
        s_f.write("Kp_H="+str(dog_data.Kp_H)+"\n")
        s_f.write("Kp_G="+str(dog_data.Kp_G)+"\n")
        s_f.write("ma_case="+str(dog_data.ma_case)+"\n")
        s_f.write("trot_cg_f="+str(dog_data.trot_cg_f)+"\n")
        s_f.write("trot_cg_b="+str(dog_data.trot_cg_b)+"\n")
        s_f.write("trot_cg_t="+str(dog_data.trot_cg_t)+"\n")
        s_f.write("Kp_TW="+str(dog_data.Kp_TW)+"\n")      
        s_f.write("in_y="+str(Yst)+"\n") #保存重心平移量
        s_f.close()      
        url_n=url_c
        dog_control.servo_init(0)

      elif value == 'hi':
        dog_data.parts[int(user_leg_num)-1][1]=dog_data.parts[int(user_leg_num)-1][1]+1
      elif value == 'hd':
        dog_data.parts[int(user_leg_num)-1][1]=dog_data.parts[int(user_leg_num)-1][1]-1
      elif value == 'si':
        dog_data.parts[int(user_leg_num)-1][2]=dog_data.parts[int(user_leg_num)-1][2]+1
      elif value == 'sd':
        dog_data.parts[int(user_leg_num)-1][2]=dog_data.parts[int(user_leg_num)-1][2]-1
      #肩部舵机调定
      elif value == 'ju':
        dog_data.parts[int(user_leg_num)-1][0]=dog_data.parts[int(user_leg_num)-1][0]+1
      elif value == 'jd':
        dog_data.parts[int(user_leg_num)-1][0]=dog_data.parts[int(user_leg_num)-1][0]-1
      elif value == 't9':
        dog_control.servo_init(1)

      # 正常页面请求
      while(req_data.find('speed')>-1 or (req_data.find('f=')==-1 and req_data.find('g0')==-1 and req_data.find('g1')==-1 and req_data.find('go')==-1 and req_data.find('gc')==-1 and req_data.find('pit=')==-1 and req_data.find('rol=')==-1 and req_data.find('yst=')==-1 and req_data.find('hgt=')==-1)):
        if (url_n=="cal.html" and req_data.find('key=ss')<0):              
          break
        with open(url_n, 'r',encoding = 'utf-8') as f:          
          while True:
            out=f.read(1024)         
            if not out:
              break
            try:                          
              cl.sendall(out.encode())
            except:pass          
        break
      if url_n=="cal.html" and  req_data.find('key=ss')==-1:
        datajson = '{"pos":["' + color_leg1 + '","' + str(dog_data.parts[0][1]) + '","' + str(dog_data.parts[0][2]) + '","' + str(dog_data.parts[0][0]) + '","' + color_leg2 + '","' + str(dog_data.parts[1][1]) + '","' + str(dog_data.parts[1][2]) + '","' + str(dog_data.parts[1][0]) + '","' + color_leg3 + '","' + str(dog_data.parts[2][1]) + '","' + str(dog_data.parts[2][2]) + '","' + str(dog_data.parts[2][0]) + '","' + color_leg4 + '","' + str(dog_data.parts[3][1]) + '","' + str(dog_data.parts[3][2]) + '","' + str(dog_data.parts[3][0]) + '"],"cfg":["' + str(dog_data.legL[0]) + '","' + str(dog_data.legL[1]) + '","' + str(dog_data.legL[2]) + '","' + str(dog_data.bodyL) + '","' + str(dog_data.bodyW) + '","' + str(dog_data.legW) + '","' + str(dog_data.speed) + '","' + str(dog_data.raiseH) + '","' + str(dog_data.trot_cg_f) + '","' + str(dog_data.trot_cg_b) + '","' + str(dog_data.trot_cg_t) + '","' + str(dog_data.Kp_H) + '","' + str(dog_data.Kp_G) + '","' + str(dog_data.Kp_TW) + '"]}'
        cl.sendall(datajson.encode())  
    cl.close()          #关闭socket
  except Exception as e:
    print(f"web except:{e}")
  

  try:  
    #命令
    if thr>=6:   #最高6
      thr=6
    elif thr<=-4:
      thr=-4
       
    if turn>=80:
      L=1;R=-1;thr=3
    elif turn<=-80:
      L=-1;R=1;thr=3
    else:
      L=1;R=1
    
    if thr==0:
      dog_cfg.spd_goal=0
      if step_state==1:
        dog_control.move(0,1,1)
      #elif abs(0-dog_cfg.spd)<=0.5:    #赋予减速过程
      else:
        dog_control.move(0,0,0)
    else:
      dog_control.move(thr,L,R)
    #print("thr:%d spd_goal:%d step_state:%d"%(thr,dog_cfg.spd_goal,step_state))
    
    dog_control.height(Hgt)
    dog_control.gesture(Pitch,Roll,Yaw,Yst)
  except Exception as ee:
    print("web conntrol except:"+str(ee))
